import sensor
import display,time
from pyb import LED,Timer,UART

#************************************外设*************************************
sensor.reset()                       # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565)  # or sensor.GRAYSCALE
sensor.set_framesize(sensor.QQVGA2)  # Special 128x160 framesize for LCD Shield.
lcd       = display.SPIDisplay()     # Initialize the lcd screen.
red_led   = LED(1)
green_led = LED(2)
blue_led  = LED(3)
red_led.on()
green_led.on()
blue_led.on()
uart      = UART(3,9600)
uart.init(9600,bits=8,parity=None,stop=1)
#************************************变量*************************************
red_point = [0,0,0,0,0]
point_count = [0,0,0,0,0]
red_threshold = [(30, 70, 30, 50, -20, 46)]
size = 128//16
height_1 = 20
height_2 = 40
track_roi = [(size*0,height_1,size*2,height_2),
             (size*2,height_1,size*2,height_2),
             (size*4,height_1,size*8,height_2),
             (size*12,height_1,size*2,height_2),
             (size*14,height_1,size*2,height_2)]

# 计算误差
def Error_calculate(data):
    if data[1] == 0 and data[2] == 1 and data[3] == 0:  # 直行
        return 0
    elif data[1] == 1 and data[2] == 1 and data[3] == 0:
        return 1
    elif data[1] == 1 and data[2] == 0 and data[3] == 0:
        return 2
    elif data[1] == 0 and data[2] == 1 and data[3] == 1:
        return 3    # -0.75
    elif data[1] == 0 and data[2] == 0 and data[3] == 1:
        return 4    # -1.5
    elif data[0] == 1 and data[1] == 0 and data[2] == 0 and data[3] == 0 and data[4] == 0:
        return 5    # 2
    elif data[0] == 0 and data[1] == 0 and data[2] == 0 and data[3] == 0 and data[4] == 1:
        return 6    # -2
    elif data[0] == 1 and data[1] == 1 and data[2] == 1 and data[3] == 1 and data[4] == 1:  # 路口
        return 7    # 3
    elif (data[0] == 1 or data[4] == 1) and (data[1] == 1 or data[2] == 1 or data[3] == 1):  # 路口
        return 7
    else:
        return 0

# 串口发送
def Uart_send(data):
    global uart
    send_data = [0x0a,0x0b]
#    for d in data:
    send_data.append(data)
    send_data.append(0x0c)
    uart.write(bytearray(send_data))
    print(send_data)

# 找红线
def Find_red_line(img):
    global red_point,red_threshold,track_roi,point_count
    red_point = [0,0,0,0,0]
    for r in range(0,len(track_roi)):
        if r == 2:
            blobs = img.find_blobs(red_threshold,roi=track_roi[r],area_threshold=1000)
        else:
            blobs = img.find_blobs(red_threshold,roi=track_roi[r],area_threshold=100)
        if blobs:
            point_count[r] += 1
            if point_count[r] >= 3:
                red_point[r] = 1
        else:
            point_count[r] = 0
        for blob in blobs:
            img.draw_rectangle(blob.rect())
            img.draw_string(blob.cx(),blob.cy(),str(blob.area()))
#        print(point_count)


# 画线
def Draw_line(img):
    global red_point,track_roi
    for t in range(0,len(track_roi)):
        #img.draw_line(track_roi[t][0],track_roi[t][1],track_roi[t][0],160)
        img.draw_rectangle(track_roi[t])
    for r in range(0,len(red_point)):
        if red_point[r] == 0:
            img.draw_rectangle(track_roi[r][0]+track_roi[r][2]//6,140,track_roi[r][2]//6*4,15,color=(255,0,0),fill=False)
        else:
            img.draw_rectangle(track_roi[r][0]+track_roi[r][2]//6,140,track_roi[r][2]//6*4,15,color=(255,0,0),fill=True)

while True:
    img = sensor.snapshot()
    Find_red_line(img)
    Uart_send(Error_calculate(red_point))
    Draw_line(img)
    lcd.write(img)
